//*****************************************************************************
//
// File Name	: ''
// Title		:
// Author		: Andrzej Borowiec
// Created		:
// Revised		:
// Version		:
// Target MCU	:
// Editor Tabs	:
//
// This code is distributed under the GNU Public License
//		which can be found at http://www.gnu.org/licenses/gpl.txt
//
//*****************************************************************************

#ifndef MO2012A_H
#define MO2012A_H

//--------------------------------------------------------------------------


#define MOTA_DIR2   PD4
#define MOTA_DIR1   PD2
#define MOTA_PWM    PD6

#define MOTB_DIR2   PD7
#define MOTB_DIR1   PB0
#define MOTB_PWM    PD5

#define MOSI        PB3
#define MISO        PB4
#define SCK         PB5

#define TX          PD1
#define RX          PD0

#define STATUS_LED  PB5

#define FORWARD  0
#define REVERSE  1

#define MAX_PWM_COUNT 255

#define LEFT  1
#define RIGHT 2
#define BOTH  0

#define NUM_OF_SENSORS 6

#define LEDR1 0
#define LEDR2 1
#define LEDR3 2
#define LEDL3 3
#define LEDL2 4
#define LEDL1 5

#define NUM_OF_CORRELATORS 5

//--------------------------------------------------------------------------
// State machine definitions

#define ARROWUP			'A'
#define ARROWDOWN		'B'
#define ARROWRIGHT		'C'
#define ARROWLEFT		'D'
//--------------------------------------------------------------------------
#define VT100_RED       91
#define VT100_GREEN     92
#define VT100_YELLOW    93
#define VT100_BLUE      94
#define VT100_MAGENTA   95
#define VT100_CYAN      96

#define ERROR_BUFFER_SIZE 20

#define STATE_TRACKING      0
#define STATE_LINE_MARKER   1
#define STATE_LINE_CROSSING 2

#define STATE_START_SEARCH  1
#define STATE_RUN           2
#define STATE_FINISH        3

//--------------------------------------------------------------------------
//Data Structures
typedef struct
{
	uint8_t  mode_of_operation;
	uint8_t  debug;
	uint8_t  max_pwm;
    int16_t  k[3];
    uint16_t  loop_delay;
} MoParameters_struct;

// Main structures
typedef struct
{
	int16_t taps[NUM_OF_SENSORS];
	int16_t calc_offset;
	int16_t offset;
} EE_RAM_CenterOfMassTaps_t;

typedef struct
{
	int16_t error;
	int16_t error_last;
	int16_t error_integral;
	uint8_t pwm[2];
	uint8_t dir[2];
	uint8_t the_state, present_state, previous_state, next_state, corr_count;

} ControlParam_struct;
typedef struct
{
	uint8_t *dataptr;		///< the physical memory address where the buffer is stored
	uint16_t size;			///< the allocated size of the buffer
	uint16_t datalength;
	uint16_t dataindex;		///< the index into the buffer where the data starts
} int16Buffer_t;

typedef struct
{
	int16_t calc_offset;
	int16_t corr_thresh[NUM_OF_CORRELATORS];
	int16_t corr_coeff_num[NUM_OF_CORRELATORS][NUM_OF_SENSORS];
} EE_RAM_CorrThreshCoeff_t;


//Functions
void InitializeVariables(void);
void RumCmdLine(void);
void idn(void);
inline void clc(void)
{
    vt100ClearScreen();
    vt100SetCursorPos(1,0);
}
void test(void);
void BlinkStatusLed(uint8_t num_of_times, uint16_t delay);

void DisplaySensors(void);
//--------------------------------------------------------------------------
void ReadSensors(void);
//Buffer Function Headers
void ErrorBufferInit(int16Buffer_t *buffer, uint8_t *start, uint16_t size);
unsigned char ErrorBufferAddToEnd(int16Buffer_t* buffer, int16_t data);
void ErrorBufferPrint(int16Buffer_t *buffer);
int16_t ErrorBufferGetValues(int16Buffer_t* buffer, uint16_t index);

void SetMotorPwm(uint8_t motor, uint8_t dir, uint16_t pwm);
void ManualDrive(void);
//--------------------------------------------------------------------------
//Shell Commands
void ShellSetMotorPwm(void);
void ShellHelp(void);
void ShellPrintBuffer(void);
void ShellAddToBuffer(void);
void ShellGetParams(void);
void ShellSetParam(void);
void ShellGetCorrelators(void);
void ShellAdc(void);
void ShellMonitor(void);
void PrintMonitorPage(void);
void PrintArray(uint8_t size, uint8_t *array, uint8_t show_sign);
void PrintArray16(uint8_t size, int16_t *array, uint8_t show_sign);
//--------------------------------------------------------------------------
//Main Loop
void PrintDebug(void);
void GetCurrentState(void);
void GetNewPwmValues(void);
void Run(void);
#endif
